#include "MergeTriangleOperator.h"
#include "OCMain/Plane.h"

namespace OCPP
{

    inline String genKey(osg::Vec3 v1, osg::Vec3 v2)
    {
        return Stringify() <<
            v1.x() << " " << v1.y() << " "<< StringConverter::formatDoubletoString(v1.z()) << " " <<
            v2.x() << " " << v2.y() << " "<< StringConverter::formatDoubletoString(v2.z());
    }

    void GltfTriangleOperator::genEdgeSet()
    {
        if (lineSet.valid())
        {
            std::map<String, String> x;
            for (auto it : mEdges)
            {
                if (it.second != "")
                {
                    StringVector sv = StringUtil::split(it.second);
                    unsigned int p1 = StringConverter::parseUnsignedInt(sv[0]);
                    unsigned int p2 = StringConverter::parseUnsignedInt(sv[1]);
                    if (p1 > p2)
                    {
                        std::swap(p1, p2);
                    }
                    osg::Vec3 v1 = mPosition->at(p1);
                    osg::Vec3 v2 = mPosition->at(p2);
                    String k = genKey(v1,v2);
                    String v = Stringify() << p1 << " " << p2;
                    if (x.find(k) == x.end())
                    {
                        x[k] = v;
                        lineSet->addElement(p1);
                        lineSet->addElement(p2);
                    }
                }
            }
            mEdges.clear();
        }
    }


    void GltfTriangleOperator::addTri(unsigned int p1, unsigned int p2, unsigned int p3)
    {
        osg::Vec3i tri(p1 + offset, p2 + offset, p3 + offset);
        if (mapRepeat3.find(tri) != mapRepeat3.end())
        {
            return;
        }

        mapRepeat3[tri] = 1;

        set->addElement(tri.x());
        set->addElement(tri.y());
        set->addElement(tri.z());

        if (nor.valid())
        {
            if (nor->size() < pos->size())
            {
                nor->resize(pos->size());
            }
            const osg::Vec3& v1 = pos->at(tri.x());
            const osg::Vec3& v2 = pos->at(tri.y());
            const osg::Vec3& v3 = pos->at(tri.z());
            osg::Vec3 nr = (v2 - v1) ^ (v3 - v1);
            nr.normalize();
            nor->at(tri.x()) = nr;
            nor->at(tri.y()) = nr;
            nor->at(tri.z()) = nr;
        }

        if (mPosition.valid() && lineSet.valid())
        {
            std::vector<unsigned int> v;
            v.push_back(tri.x());
            v.push_back(tri.y());
            v.push_back(tri.z());
            std::sort(v.begin(), v.end());
            unsigned int p1 = v[0];
            unsigned int p2 = v[1];
            unsigned int p3 = v[2];

            std::map<std::string, std::string>& edges = mEdges;
            osg::Vec3Array* pos = mPosition.get();
            
            auto findEdge = [&pos, &edges](unsigned int p1, unsigned int p2, unsigned int p3)
            {
                osg::Vec3 v1 = pos->at(p1);
                osg::Vec3 v2 = pos->at(p2);
                osg::Vec3 v3 = pos->at(p3);
                //String k1 = osgEarth::Stringify() << v1 << " " << v2;
                String k1 = genKey(v1,v2);
                auto it = edges.find(k1);
                if (it == edges.end())
                {
                    edges[k1] = (Stringify() << p1 << " " << p2 << " " << p3);
                }
                else if (!it->second.empty())
                {
                    Vector3 p333 = Vec3ToVector3(pos->at(p3));
                    StringVector sv = StringUtil::split(it->second);
                    osg::Vec3 v11 = pos->at(StringConverter::parseUnsignedInt(sv[0]));
                    osg::Vec3 v22 = pos->at(StringConverter::parseUnsignedInt(sv[1]));
                    osg::Vec3 v33 = pos->at(StringConverter::parseUnsignedInt(sv[2]));
                    OCPP::Plane plane(Vec3ToVector3(v11), Vec3ToVector3(v22), Vec3ToVector3(v33));
                    if (Math::floatEqual(plane.getDistance(p333), 0.0, 0.0001))
                    {
                        edges[k1] = "";
                    }
                }
            };

            findEdge(p1, p2, p3);
            findEdge(p2, p3, p1);
            findEdge(p1, p3, p2);

            findEdge(p3, p2, p1);
            findEdge(p3, p1, p2);
            findEdge(p2, p1, p3);
        }
    }
}

